#include <ros/ros.h> 
#include <serial/serial.h>  //ROS已经内置了的串口包 
#include <std_msgs/String.h> 
#include <std_msgs/Empty.h> 
#include "serialport/ALL_UAVs_LLA.h"
// #include "serialport/geo.h"
#include <string>
#include <time.h>
#include <sstream>
#include "serialport/data_make.h"

using namespace std;
serial::Serial ser; //声明串口对象 
uint8_t split(double data, int position);    //将64位的double变成uint32_t再变成uint8_t
double merge(uint8_t first, uint8_t second, uint8_t third, uint8_t fourth); //将4个uint8_t转成uint32_t,再变成double
uint8_t data_crc(uint8_t *p_data, int len_data);   //接收校验
int Blen_child = 16;    //子节点的数据长度
int Blen_host  = 52;    //主节点的数据长度
int Blen_stop  = 5;     //子节点信息发送许可证---信息
uint8_t recv_buff[40];
uint8_t send_buff[52];
uint8_t send_buff_last[52];
uint8_t stop_buff[5];
double longitude = 0, latitude = 0, altitude = 0;
int fail_num = 0,uav_recv_all = 0;
int len_send, lend_recv;
int uav_ID, send_stop;

struct uav_MSG
{
    double longitude;
    double latitude;
    double altitude;
};
uav_MSG uav={},uav1={},uav2={},uav3={},uav4={},uav1_last={},uav2_last={},uav3_last={},uav4_last={};

int main (int argc, char** argv) 
{ 
    //初始化节点 
    ros::init(argc, argv, "host_node"); 
    //声明节点句柄 
    ros::NodeHandle nh; 
    ros::Rate loop_rate(20);     //设置循环频率，括号内参数即为频率

    try 
    { 
        //设置串口属性，并打开串口 
        ser.setPort("/dev/ttyUSB0"); 
        ser.setBaudrate(115200); 
        serial::Timeout to = serial::Timeout::simpleTimeout(1000); 
        ser.setTimeout(to); 
        ser.open(); 
    } 
    catch (serial::IOException& e) 

    { 
        ROS_ERROR_STREAM("Unable to open port "); 
        return -1; 
    } 
    //检测串口是否已经打开，并给出提示信息 
    if(ser.isOpen()) 
    { 
        ROS_INFO_STREAM("Serial Port initialized"); 
    } 
    else 
    { 
        return -1; 
    }

    struct SINFO
    {
        int send_num;
        int send_r_num;
        float send_r_rate;
        int recv_num;
        int recv_r_num;
        float recv_r_rate;
    };
    SINFO s_info = {};

    //消息的定义
    //2号消息：子节点能否发送信息的许可
    stop_buff[0] = 0xa4;
    stop_buff[1] = 0xf1;
    stop_buff[2] = 0x2;
    stop_buff[3] = 0xf;     //现在是4架飞机，所以只用低4位。全为1代表都可发送
    stop_buff[4] = data_crc(stop_buff,Blen_stop-1);
    
    //计时相关参数
    bool timeout, loop_state, send_state =1,recv_state = 0;
    clock_t start_time=0,end_time=0;
    int time_dif=0;

    while(ros::ok()) 
    {
        cout << endl;
        // recv_msg.uav_ID |=0x1000;
        //更新自身信息****************************************************
        uav4.longitude = 144.131513565;
        uav4.latitude = 44.2656438;
        uav4.altitude = 4.25451348;
        uav_ID |=0x1000;
        //接收信息****************************************************
        lend_recv = ser.read(recv_buff,ser.available()); 
        if (lend_recv) 
        {
            s_info.recv_num++;
            cout << "lend_recv:" << lend_recv <<endl;
            for (int i =0;i<lend_recv;i++)
            {
                printf("%#x",recv_buff[i]);
            }
            cout << endl;
        }
        //消息的校验&分类存储
         for (int i=0; i<lend_recv; i++)
        {
            if (recv_buff[i] == 0xa4 && (recv_buff[i+1] == 0xc1|| recv_buff[i+1] == 0xc2 || recv_buff[i+1] == 0xc3))
            {
                if (recv_buff[i+2] == 0x1 && i+Blen_child-1<lend_recv)
                {
                    // 校验位检验
                    if (data_crc(&recv_buff[i],Blen_child-1) == recv_buff[i+Blen_child-1])
                    {
                        s_info.recv_r_num++;
                        uav.longitude = merge(recv_buff[i+3],recv_buff[i+4],recv_buff[i+5],recv_buff[i+6]);
                        uav.latitude =     merge(recv_buff[i+7],recv_buff[i+8],recv_buff[i+9],recv_buff[i+10]);
                        uav.altitude =     merge(recv_buff[i+11],recv_buff[i+12],recv_buff[i+13],recv_buff[i+14]);
                        int uav_num = recv_buff[i+1] - 0xc0;
                        switch (uav_num)
                        {
                            case 1:{uav1 = uav;uav_ID |=0x1;} break;
                            case 2:{uav2 = uav;uav_ID |=0x10;} break;
                            case 3:{uav3 = uav;uav_ID |=0x100;} break;
                        }
                        cout << uav_num << "号发来信息" << endl;
                        cout << "uav:" << uav.longitude  << "  " << uav.latitude  << "  " << uav.altitude  <<endl;
                        // uav = {};
                        break;
                    }
                    else
                    {
                        fail_num++;
                        // break;
                    }
                }
            }
        }
        memset(recv_buff,0,sizeof(recv_buff));
        printf("uav_ID:%#x\n",uav_ID);
        //让某机停止发消息********************************************************************************
        // send_stop = uav_ID;
        // send_stop &= 0x111; //让最高位先为0
        // stop_buff[3] = 0xf;     // 默认让所有飞机都可发消息
        // // stop_buff[3] = ~uav_ID; 
        // while (send_stop != 0)
        // {
        //     cout << "让某机停止" << endl;
        //     if (send_stop & 0x1)    //第一位判断
        //     {
        //         stop_buff[3] &= 0xe;
        //         send_stop     &= 0x1110;
        //     }
        //     if (send_stop & 0x10)   //第二位判断
        //     {
        //         stop_buff[3] &= 0xd;
        //         send_stop     &= 0x1101;
        //     }
        //     if (send_stop & 0x100)  //第三位判断
        //     {
        //         stop_buff[3] &= 0xb;
        //         send_stop     &= 0x1011;
        //     }
        // }
        // stop_buff[4] = data_crc(stop_buff,Blen_stop-1);
        // len_send = ser.write(stop_buff,Blen_stop);
        // s_info.send_num++;
        // if (len_send == Blen_stop)
        // {
        //     s_info.send_r_num++;
        // }
        //发送信息***************************************************************************
        if (uav_ID == 0x1111)
        {
            uav_recv_all++;
            cout << "uav1:" << uav1.longitude  << "  " << uav1.latitude  << "  " << uav1.altitude  <<endl;
            cout << "uav2:" << uav2.longitude  << "  " << uav2.latitude  << "  " << uav2.altitude  <<endl;
            cout << "uav3:" << uav3.longitude  << "  " << uav3.latitude  << "  " << uav3.altitude  <<endl;
            cout << "uav4:" << uav4.longitude  << "  " << uav4.latitude  << "  " << uav4.altitude  <<endl;
            send_buff[0] = 0xa4;
            send_buff[1] = 0xf1;
            send_buff[2] = 0x1;
            send_buff[3] = split(uav1.longitude,1); send_buff[4] = split(uav1.longitude,2); send_buff[5] = split(uav1.longitude,3); send_buff[6] = split(uav1.longitude,4);
            send_buff[7] = split(uav1.latitude,1);     send_buff[8] = split(uav1.latitude,2);     send_buff[9] = split(uav1.latitude,3);     send_buff[10] = split(uav1.latitude,4);
            send_buff[11] = split(uav1.altitude,1);     send_buff[12] = split(uav1.altitude,2);     send_buff[13] = split(uav1.altitude,3);     send_buff[14] = split(uav1.altitude,4);
            send_buff[15] = split(uav2.longitude,1); send_buff[16] = split(uav2.longitude,2); send_buff[17] = split(uav2.longitude,3); send_buff[18] = split(uav2.longitude,4);
            send_buff[19] = split(uav2.latitude,1);     send_buff[20] = split(uav2.latitude,2);     send_buff[21] = split(uav2.latitude,3);     send_buff[22] = split(uav2.latitude,4);
            send_buff[23] = split(uav2.altitude,1);     send_buff[24] = split(uav2.altitude,2);     send_buff[25] = split(uav2.altitude,3);     send_buff[26] = split(uav2.altitude,4);
            send_buff[27] = split(uav3.longitude,1); send_buff[28] = split(uav3.longitude,2); send_buff[29] = split(uav3.longitude,3); send_buff[30] = split(uav3.longitude,4);
            send_buff[31] = split(uav3.latitude,1);     send_buff[32] = split(uav3.latitude,2);     send_buff[33] = split(uav3.latitude,3);     send_buff[34] = split(uav3.latitude,4);
            send_buff[35] = split(uav3.altitude,1);     send_buff[36] = split(uav3.altitude,2);     send_buff[37] = split(uav3.altitude,3);     send_buff[38] = split(uav3.altitude,4);
            send_buff[39] = split(uav4.longitude,1); send_buff[40] = split(uav4.longitude,2); send_buff[41] = split(uav4.longitude,3); send_buff[42] = split(uav4.longitude,4);
            send_buff[43] = split(uav4.latitude,1);     send_buff[44] = split(uav4.latitude,2);     send_buff[45] = split(uav4.latitude,3);     send_buff[46] = split(uav4.latitude,4);
            send_buff[47] = split(uav4.altitude,1);     send_buff[48] = split(uav4.altitude,2);     send_buff[49] = split(uav4.altitude,3);     send_buff[50] = split(uav4.altitude,4);
            send_buff[51] = data_crc(send_buff,Blen_host-1);
            len_send = ser.write(send_buff,Blen_host); 
            s_info.send_num++;
            // ros::Duration(0.05).sleep(); //sleep for 50 ms
            if (len_send == Blen_host)
            {
                cout << "send successful!" << endl;
                s_info.send_r_num++;
            }
            //事后工作（清除、更新上次信息）***************************************************************************
            uav_ID = 0;
            memset(send_buff,0,sizeof(send_buff));
            uav1_last = uav1; uav2_last = uav2; uav3_last = uav3; uav4_last = uav4;
            uav1 = {}; uav2 = {}; uav3 = {}; uav4 = {}; uav = {};
        }
        //处理ROS的信息，比如订阅消息,并调用回调函数 
        // ros::spinOnce(); 
        // loop_state = loop_rate.sleep();
        // cout << "loop_state is " << loop_state << endl;
        cout << "uav_recv_all:" << uav_recv_all << endl;
        cout << "fail_num:" << fail_num << endl;
        cout << "send_num: " << s_info.send_num << "\t" << "send_r_num: " << s_info.send_r_num << endl;
        cout << "recv_num: " << s_info.recv_num << "\t" << "recv_r_num: " << s_info.recv_r_num << endl;
        cout <<  endl;
    }
    return 0;
}

// uint8_t split(double data, int position)
// {
//     uint8_t result = 0;
//     uint32_t data_32 = (uint32_t)(data*1e7);
//     switch (position)
//     {
//         case 1:{result = (uint8_t)(data_32 >> 24);return result;}
//         break;
//         case 2:{result = (uint8_t)((data_32 & 0x00ff0000) >> 16);return result;}
//         break;
//         case 3:{result = (uint8_t)((data_32 & 0x0000ff00) >> 8);return result;}
//         break;
//         case 4:{result = (uint8_t)(data_32 & 0x000000ff);return result;}
//         break;
//     }
// }

// double merge(uint8_t first, uint8_t second, uint8_t third, uint8_t fourth)
// {
//     uint32_t temp = 0;
//     double result = 0;
//     temp = ((uint32_t)first<<24);
//     temp |= ((uint32_t)second<<16);
// 	temp |= ((uint32_t)third<<8);
// 	temp |= ((uint32_t)fourth); 
//     result = (double)(temp / 10000000.0);
// }

// uint8_t data_crc(uint8_t *p_data, int len_data)
// {
//     uint8_t result = 0;
//     while (len_data--)
//     {
//         result = result ^ (*p_data++);
//     }
//     return result;
// }